#!/usr/bin/env python
#coding:utf-8

import struct 
import binascii
import serial
import math
from datetime import datetime
import time



class HDDiscoveryQ2:
	def __init__(self):
		self.q2 = struct.Struct('<3sbh4hBHB')
		self.type	   = 'dq2'
		self.move_mode = 0
		self.start_dir = 0
		self.speed	   = 0	#if move_mode != 3, use speed!
		self.flm	   = 0	#front left  motor
		self.frm	   = 0	#front right motor
		self.blm	   = 0	#back  left  motor
		self.brm	   = 0	#back  right motor
		self.unused	   = 0
		self.verge	   = 0
		self.end	   = 0xaa

	def pack(self):

		if self.move_mode == 3:
			#这里的代码是硬件相关，四个轮子（下标从0开始）
			#物理位置机器转动方向如图所示
			#  3/\    2\/
			#  					
			#
			#  0/\    1\/
			m=4*[0]
			m[3] = -self.flm
			m[2] =  self.frm
			m[0] = -self.blm
			m[1] =  self.brm
			data = self.q2.pack( self.type, self.move_mode, 0, \
								 m[0],m[1],m[2],m[3], \
								 self.unused, 0, 0 )
		
			checksum = 0
			for i in range( len(data) ):
				x, = struct.unpack('<B', data[i])
				checksum+=int(x)
			self.verge = checksum
			return self.q2.pack( self.type, self.move_mode, 0, \
								 m[0],m[1],m[2],m[3], \
								 self.unused, self.verge, self.end )
		#if move_mode != 3:
		#兼容以前老算法
		data = self.q2.pack( self.type, self.move_mode, self.start_dir, \
							 self.speed, 0, 0, 0, \
							 self.unused, 0, 0 )
		
		checksum = 0
		for i in range( len(data) ):
			x, = struct.unpack('<B', data[i])
			checksum+=int(x)
		self.verge = checksum
		return self.q2.pack( self.type, self.move_mode, self.start_dir, \
							 self.speed, 0, 0, 0, \
							 self.unused, self.verge, self.end )

if __name__ == '__main__':
	global q2
	global serial_port
	serial_port = serial.Serial( '/dev/ttyUSB0', 9600, timeout=2 )
	
	q2 = HDDiscoveryQ2()
	q2.move_mode = 3
	q2.flm = 100
	q2.frm = 100
	q2.blm = 1000
	q2.brm = 1000
	serial_port.write( q2.pack() )
	serial_port.flush()
	
	exit(0)
	
	q2.speed = 100
	q2.start_dir = 90
	
	print 'size of each command:', q2.q2.size
	
	#f = open( 'test' , "wb" )
	#f.write( q2.pack() )
	
	interval = 1000  #in millisecond
	total = 0	   #max >=1000ms (1s)
	
	#停止命令
	q2_2 = HDDiscoveryQ2()
	q2_2.move_mode = 1
	i=0
	while i<10:
		print 'stop'
		time.sleep(0.2)
		serial_port.write( q2_2.pack() )
		serial_port.flush()
		i+=1
	
	#往前逐步加速走
	i=0
	q2.move_mode = 0
	while i<20:
		time.sleep(0.2)
		print '11==',  datetime.now()
		q2.speed = i*100
		serial_port.write( q2.pack() )
		serial_port.flush()
		
		i+=1
	
	print'++++'
	
	#逐步减速
	i = 0
	q2.move_mode = 0
	while i<=20:
		time.sleep(0.2)
		print '22==',  datetime.now()
		q2.speed = 2000 - i*100
		serial_port.write( q2.pack() )
		serial_port.flush()
		

		i+=1
	
	#停止
	q2.move_mode = 2
	serial_port.write( q2.pack() )
	serial_port.flush()
	
	
	print 'exit==++', datetime.now()
	exit(0)
	
	print 'exit===='
	
	time.sleep(5)
	
	print '===='
	q2.move_mode = 0
	serial_port.write( q2.pack() )
	
	
	exit(0)
	
	before = datetime.now()
	print before
	
	while True:
		duration = datetime.now() - before

		if total >=3*1000: 
			break
			
		if ( duration.microseconds/1000 ) > interval:
			serial_port.write( q2.pack() )
			before = datetime.now()
			print 'aa:', before
			total += interval
			
	print 'times:%d' % ( total/interval )
	while True:
		line = serial_port.readline()
		print line

